#include "SingleDispatchParams.h"
#include "Main.h"

SingleDispatchParams::SingleDispatchParams()
: m_speed(pSysConfig->nSpeed1, pSysConfig->nSpeed2, pSysConfig->nSpeed3, pSysConfig->nSpeed4)
, m_iUnloadDirection(-1)
, m_iRechargeLine(1)
, m_fUnloadZoneLength(14000.0f)
, m_fUnloadCargoGap(200.0f)
, m_fDecelerationDistance(4000.0f)
, m_fChangeLaneDecelerationDistance(1000.0f)
, m_fClearZoneLength(3000.0f)
, m_fLoadPointToAuxiliaryLaneDistance(3000.0f)
, m_fOnlineBlindDistance(1500.0f)
, m_fErrDistance(200.0f)
, m_pid(pSysConfig->nNavP, pSysConfig->nNavI, pSysConfig->nNavD)
{
	float pidTerm[] = {0.0f, 0.0f, 0.0f, 400.0f, -400.0f};
	for (int i=0; i<sizeof(pidTerm)/sizeof(float); i++)
     m_fInitPIDTerminal[i] = pidTerm[i];
	
	float pathLen[] = {0.0f, 0.0f, -4810.0f, -6000.0f, -8000.0f, -10000.0f, -12000.0f};
	for (int i = 0; i <= 6; ++i)
		for (int j = 0; j <= 6; ++j)
		{
			m_fPathLenMatrix[i][j] = (i == 0) ?
				pathLen[j] :
				pathLen[j] - pathLen[i];
		}
}
